-- 程序主入口
-- 开发日期：20200106
-- 说明：本程序是三合通德#8号机 打卡簧机程序
-- 版本号：V1.0
-- 历史修改记录
-- 20210107：测试自动程序逻辑，修改复位程序逻辑,PLC核对自动程序,完成 

print("Main Thread Start!!!")

-- 机器人运行状态
RbtState = 0

-- 机器人停止状态
RbtStop = 101
-- 机器人运行状态
RbtRunning = 102
-- 机器人暂停状态
RbtPause = 103

-- 机器人自动模式状态
bAuto = false
-- 机器人手动模式状态
bManual = false


-- 描述: 初始化IO
function ResetIO()
	
	IO.Set(DO_READY,0)
	IO.Set(DO_ALARM,0)
	
	IO.Set(DO_RUNNING,0)
	IO.Set(DO_PAUSE,0)
	IO.Set(DO_STOP,1)
	
	IO.Set(DO_MOTIONFINISH,0)
	IO.Set(DO_HOMEFINISH,0)
	
	IO.Set(DO_READY,1)
	
	-- 由于cell的DO需要1ms传输时间.增加延迟时间保证所有DO重置完成.
	Thread.Sleep(0.05)

end

-- 描述: 初始化参数
function ResetParam()

	-- 设置机器人速度
	Robot.Speed(1,100)
	
	-- 定义PLC端存储机器人点位的数量
	GInt[1]=20
	
	-- 定义判断机器人是否处于Jog模式
	GBool[64]=false

	RbtState = RbtStop

end

-- 线程定义
-- ================================================
-- 定义PLC通讯线程
PLCComThd = Thread.New(PLCCom)()

-- 定义实时获取机器人坐标(笛卡尔坐标&关节坐标)线程
UpdateRbtSttThd = Thread.New(UpdateRbtStt)()

-- 定义手动功能线程
ManualMotionThd = Thread.New(ManualMotion)()

-- 定义自动功能线程
AutoMotionThd = Thread.New(AutoMotion)()
-- ================================================

--OpenErrorThd = Thread.New(OpenError)()

-- 参数初始化以及IO初始化
-- ================================================
ResetIO()
ResetParam()
-- ================================================

-- 线程启动
-- ================================================
-- 启动实时获取机器人坐标(笛卡尔坐标&关节坐标)线程
UpdateRbtSttThd:Start()

-- 启动PLC通讯线程
PLCComThd:Start()

--OpenErrorThd:Start()
-- ================================================

::ReStart::
try{
	function()
		-- 状态机扫描线程
		while true do
			
			System.CheckError()

			-- 启动
			-- 启动信号为True; 模式为自动模式(检查GBool[1]共享变量值); 机器人报警信号为False
			if IO.Get(DI_START) == 1 and GBool[1] == false and IO.Get(DO_ALARM) == 0 then

				-- 停止手动功能线程
				if ManualMotionThd.Status == "running" or ManualMotionThd.Status == "paused" or ManualMotionThd.Status == "waiting" then 

					print("自动模式下:停止手动功能线程")
					ManualMotionThd:Cancel()	
					Thread.Sleep(0.5)
				end

				-- 启动自动功能线程
				if RbtState == RbtStop then
					
					IO.Set(DO_RUNNING,1)
					IO.Set(DO_PAUSE,0)
					IO.Set(DO_STOP,0)
				
					print("自动模式下:启动自动功能线程")
					System.Restart()
					AutoMotionThd:Start()

					RbtState = RbtRunning
					
					Thread.Sleep(0.5)

				end

				bAuto = true  
				bManual = false
			
			end
			
			-- 继续
			-- 继续信号为True; 模式为自动模式(检查GBool[1]共享变量值); 机器人报警信号为False
			if IO.Get(DI_RESUME) == 1 and GBool[1] == false and IO.Get(DO_ALARM) == 0 then

				-- 停止手动功能线程
				if ManualMotionThd.Status == "running" or ManualMotionThd.Status == "paused" or ManualMotionThd.Status == "waiting" then 

					print("自动模式下:停止手动功能线程")
					ManualMotionThd:Cancel()	
					Thread.Sleep(0.5)
				end
			
				-- 继续自动功能线程
				if RbtState == RbtPause then 
					
					IO.Set(DO_RUNNING,1)
					IO.Set(DO_PAUSE,0)
					IO.Set(DO_STOP,0)
				
					print("自动模式下:继续自动功能线程")	
					AutoMotionThd:Resume()

					RbtState = RbtRunning

				end
				
				bAuto = true  
				bManual = false
				
			end
			
			-- 暂停
			-- 暂停信号为True; 模式为自动模式(检查GBool[1]共享变量值); 机器人报警信号为False
			if IO.Get(DI_PAUSE) == 1 and GBool[1] == false and IO.Get(DO_ALARM) == 0 then

				-- 停止手动功能线程
				if ManualMotionThd.Status == "running" or ManualMotionThd.Status == "paused" or ManualMotionThd.Status == "waiting" then 

					print("自动模式下:停止手动功能线程")
					ManualMotionThd:Cancel()	
					Thread.Sleep(0.5)
				end
			
				-- 暂停自动功能线程
				if RbtState == RbtRunning then 
					
					IO.Set(DO_RUNNING,0)
					IO.Set(DO_PAUSE,1)
					IO.Set(DO_STOP,0)
				
					print("自动模式下:暂停自动功能线程")	
					AutoMotionThd:Pause()

					RbtState = RbtPause

				end
				
				bAuto = true  
				bManual = false
				
			end
			
			-- 停止
			-- 停止信号为True; 模式为自动模式(检查GBool[1]共享变量值); 机器人报警信号为False
			if IO.Get(DI_STOP) == 1 and GBool[1] == false and IO.Get(DO_ALARM) == 0 then

				-- 停止手动功能线程
				if ManualMotionThd.Status == "running" or ManualMotionThd.Status == "paused" or ManualMotionThd.Status == "waiting" then 

					print("自动模式下:停止手动功能线程")
					ManualMotionThd:Cancel()	
					Thread.Sleep(0.5)
				end
			
				-- 停止自动功能线程
				if RbtState == RbtRunning or RbtState == RbtPause then 
					
					IO.Set(DO_RUNNING,0)
					IO.Set(DO_PAUSE,0)
					IO.Set(DO_STOP,1)
				
					print("自动模式下:停止自动功能线程")	
					AutoMotionThd:Cancel()
					Thread.Sleep(0.5)
					RbtState = RbtStop

				end
				
				bAuto = false  
				bManual = false
				
			end
			
			-- 模式为自动模式(检查GBool[1]共享变量值); 机器人报警信号为False
			if  GBool[1] == false and IO.Get(DO_ALARM) == 0 then

				-- 暂停手动功能线程
				if ManualMotionThd.Status == "running" or ManualMotionThd.Status == "waiting" then 
					print("自动模式下:暂停手动功能线程")
				
					bAuto = true  
					bManual = false
				
					ManualMotionThd:Pause()	
					Thread.Sleep(0.5)
				end

			end

			-- 模式为手动模式(检查GBool[1]共享变量值); 机器人报警信号为False
			if GBool[1] == true and IO.Get(DO_ALARM) == 0 then

				-- 停止自动功能线程
				if AutoMotionThd.Status == "running" or AutoMotionThd.Status == "paused" or AutoMotionThd.Status == "waiting" then 
					print("手动模式下:停止自动功能线程")
					AutoMotionThd:Cancel()
					Thread.Sleep(0.5)
				end 

				-- 启动 & 继续手动功能线程
				if bManual == false then
					
					-- 关闭复位完成信号
					IO.Set(DO_HOMEFINISH,0)
				
					IO.Set(DO_RUNNING,0)
					IO.Set(DO_PAUSE,0)
					IO.Set(DO_STOP,1)
					
					RbtState = RbtStop

					if ManualMotionThd.Status == "paused" then

						print("手动模式下:继续手动功能线程")

						ManualMotionThd:Resume()

					else

						print("手动模式下:启动手动功能线程")

						System.Restart()
						
						ManualMotionThd:Start()

					end

					Thread.Sleep(0.5)

				end

				bAuto = false  
				bManual = true 

			end

			-- 当机器人报警时,停止自动功能线程/手动功能线程
			if IO.Get(DO_ALARM) == 1 and (bAuto == true or bManual == true) and (GBool[64] == false)then

				-- 停止自动功能线程
				if AutoMotionThd.Status =="running" or AutoMotionThd.Status =="paused" or AutoMotionThd.Status == "waiting" then 
					AutoMotionThd:Cancel()
					Thread.Sleep(0.5)
				end 
				
				-- 停止手动功能线程
				if ManualMotionThd.Status =="running" or ManualMotionThd.Status =="paused" or ManualMotionThd.Status == "waiting" then
					ManualMotionThd:Cancel()
					Thread.Sleep(0.5)
				end 
				
				IO.Set(DO_RUNNING,0)
				IO.Set(DO_PAUSE,0)
				IO.Set(DO_STOP,1)
				
				IO.Set(DO_READY,0)

				RbtState = RbtStop

				bAuto = false
				bManual = false

				print("机器人报警:停止手动功能线程&停止自动功能线程")	

			end

			-- 清除报警
			if IO.Get(DI_RESET) == 1 and IO.Get(DO_ALARM) == 1 then 
				print("清除报警信号")
				
				IO.Set(DO_ALARM,0)
				GDouble[3]=0
				
				IO.Set(DO_READY,1)
				
			elseif IO.Get(DI_RESET) == 1 and IO.Get(DO_ALARM) == 0 then 

				GDouble[3]=0

			end

			Thread.Sleep(0.1)

		end
	end,
		
		catch = function(errors)
			
			-- 打印报错模块名称
			print("MAIN.Error")

			-- 打印报错代码所在文件名称
			print("file   :", errors.file)
			-- 打印报错代码行数
			print("line   :", errors.line)
			-- 打印报错信息
			print("message:", errors.message)
			-- 打印报错代码
			print("code   :", errors.code)
			-- 打印报错机器人
			print("robot  :", errors.robot)
			-- 打印报错轴数
			print("axis   :", errors.axis)
			
			ErrorCode = errors.code
			
			if ErrorCode ~= nil then
				ErrorCode = tonumber(errors.code)
			else
				ErrorCode = -20003
			end
			
			IO.Set(DO_HOMEFINISH,0)	--回零完成信号置0
			
			GDouble[3]=ErrorCode
			IO.Set(DO_ALARM,1)

		end,
			finally = function(ok,errors)
		
end
	}
	
goto ReStart	